import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3d class. Robots runs along all walls left sided . Bang-bang
 * controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller4d extends RobotController {

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	private boolean wallFound = false;

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		if (!wallFound) {
			// solange geradeaus fahren, bis wir eine Wand gefunden haben
			setMotorSpeeds(9, 9);
			if ((getDistanceValue(2) > 60) || (getDistanceValue(3) > 60)) {
				wallFound = true;
			}
		} else {

			if (getDistanceValue(0) > 100) {
				// links wird es eng!
				setMotorSpeeds(5, 4);
				wallFound = true;
			} else if (getDistanceValue(0) < 10) {
				// zu weit weg von der Wand
				setMotorSpeeds(4, 5);
			} else {
				// Vollgas!
				setMotorSpeeds(9, 9);
			}

			if (getDistanceValue(1) < 10) {
				// Vorne links ist keine Wand mehr! Leichte Drehung nach links
				setMotorSpeeds(1, 5);
			}

			if ((getDistanceValue(2) > 60) || (getDistanceValue(3) > 60)) {
				// Wand voraus! Volle Drehung nach rechts!
				setMotorSpeeds(5, -5);
				wallFound = true;
			}
		}
	}
}
